// Dual motor control based on distance

#include <SoftwareSerial.h>
//set the pins to connect to the TX and RX (crossed) of the time of flight board to
SoftwareSerial mySerial(15, 16); // RX, TX

//Define variables
int LED = 4;        //LED on pin 6
int INA1 = 10;      //h-bridge INA1 on pin 12
int INA2 = 11;      //h-bridge INA2 on pin 13
int PWMA = 0;       //h-bridge PWMA on pin 2
int INB1 = 12;      //h-bridge INB1 on pin 14
int INB2 = 13;      //h-bridge INB2 on pin 15
int PWMB = 1;       //h-bridge PWMB pin 3
int velocity = 127; //set motor speed between 0-255
int delayTime = 1;  //millisecond
int distance = 0;   //initialize variable to store distance later

void setup() {
  //begin hardware serial communication to PC
  //must have same baude rate as other board
  Serial.begin(115200);   
  //set microprocessor pin modes
  pinMode(LED, OUTPUT);
  pinMode(INA1, OUTPUT);
  pinMode(INA2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(INB1, OUTPUT);
  pinMode(INB2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  //begin and set the data rate for the SoftwareSerial port
  mySerial.begin(115200);
  Serial.println("test");

  //turn LED on
  digitalWrite(LED, HIGH); 
}

void loop() {
  mySerial.listen();
  //read distance from time of flight board
  if (mySerial.available()) {
    Serial.println("my serial is connected");
    //read distances from pins 15 and 16 (RX,TX) 
    //which are connected to other board TX and RX (crossed)
    distance = mySerial.parseInt();   
    //parseInt looks for the next valid integer in the incoming serial.
    Serial.println(distance); //prints distances to computer
  }
  //test hardware serial to see if we can input values and store them 
  if (Serial.available()) {
    distance = Serial.parseInt();
    Serial.println(distance); //prints distances to computer
  }
  
  if ( distance > 500) {
    //power on motor A
  digitalWrite(INA1,HIGH); 
  digitalWrite(INA2,LOW);
  analogWrite(PWMA,velocity); 
    //power on motor B
  digitalWrite(INB1,LOW); 
  digitalWrite(INB2,LOW); 
  analogWrite(PWMB,velocity);
  }
  else {
    //power off motor A  
  digitalWrite(INA1,LOW);
  digitalWrite(INA2,LOW); 
  analogWrite(PWMA,velocity);
    //power off motor B
  digitalWrite(INB1,LOW); 
  digitalWrite(INB2,LOW); 
  analogWrite(PWMB,velocity);
  }
}